function [ mpc ] = carMPC(sys, x, u, k, param )
[~,Y]=ode2(@(t,y)carFceSol(t,y,u,param),0:sys.Ts:sys.Ts*(param.Tp-1),x);
Y(isnan(Y))=0;
%Y=Y(2:end,:);
A=sys.A;
B=sys.B;
C=sys.C;
sys_order=size(A,2);
input_order=size(B,2);
d=zeros((param.Tp)*sys_order+input_order,1);
D=zeros(param.Tp*sys_order,length(d));
for i=2:param.Tp;
    in=(i-1)*sys_order+1;
    D(in:in+sys_order-1,in:in+sys_order-1)=eye(sys_order);
    j=1;
    jn=(j-1)*sys_order+1;
    D(in:in+sys_order-1,jn:jn+sys_order-1)=-A^(i-1);
    pomoc=zeros(size(B));
    for l=1:(i-1)
        pomoc=pomoc-A^(l-1)*B;
    end
    D(in:in+sys_order-1,end-input_order+1:end)=pomoc;
end

ref=param.ref(:,k:k+param.Tp-1);
[m,n]=size(Y);
Y=Y.';
d(1:(param.Tp)*sys_order)=Y(1:m*n);
d(end-input_order+1:end)=u;
[m,n]=size(ref);
refCol=ref(1:m*n);
refCol=refCol.'-D*d;
refT=zeros(m,n);
refT(1:m*n)=refCol(1:m*n);
refT=refT.';
mpc=InitMPC(sys,diag(param.diagQ),diag(param.diagR),diag(param.diagH));
mpc=MPCSampling(mpc,param.Tp,param.Tvz,blockMatrix(sys,param.Tvz,param.Block));
mpc=MPCHardBounds(mpc,createBounds(5,param.Tvz,param.lb,param.boundLocation,-1),createBounds(5,param.Tvz,param.ub,param.boundLocation,1));
mpc=MPCSoftBounds(mpc,createBounds(6,param.Tp,param.lb_w,[],-1),createBounds(6,param.Tp,param.ub_w,[],1),selectSoftBounds(length(sys.A),param.Tp,param.kroky,param.promenne));
mpc=addMPCstate(mpc,x);
mpc=addMPCcontrol(mpc,u);
mpc=addMPCref(mpc,refCol);
end